/*
 * platform.h
 *
 *  Created on: Dec 4, 2012
 *      Author: michiel
 */

#ifndef PLATFORM_H_
#define PLATFORM_H_

#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <geometry_msgs/Twist.h>

#include "topic/topic.h"

class Platform
{
  protected:
	ros::NodeHandle nh_;
	ros::Subscriber vel_sub_;

	C3mxlROS *left_motor_, *right_motor_;
	double wheel_diameter_, wheel_base_;

  protected:
	/// Called when a new velocity command is published
	/**
	 * Sends the new velocity to the base motors
	 * \param msg Pointer to geometry_msgs/Twist message, containing the linear and angular velocities, in [m/s] and [rad/s] respectively.
	 * \note Since the base is nonholonomic, only linear velocities in the x direction and angular velocities around the z direction are supported.
	 */
	void velocityCallback(const geometry_msgs::Twist::ConstPtr &msg);

  public:
	Platform() : nh_("~")
	{
	  init();
	}

	~Platform()
	{
	  delete left_motor_;
	  delete right_motor_;

	  nh_.shutdown();
	}

	/// Initialize the base motors
	/** \note Called during construction */
	void init();

	/// Await and process commands
	void spin();
};


#endif /* PLATFORM_H_ */
